﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Net;
using System.Net.Sockets;
using System.IO;
using System.Xml;
using System.Xml.Serialization;
using System.Threading;
using System.ComponentModel;
using System.IO.Ports;
using Phidgets;
using Phidgets.Events;
using System.Windows.Forms;
using System.Diagnostics;

namespace Simple_RCU
{
    public class Receiver
    {
        enum Dumping { All, NoDump, NoReturn }
        int listenPort;
        private BackgroundWorker roboteqs;
        private volatile bool shouldStop;
        InterfaceKit ikit;

        Servo phidgetMotors;
        static readonly object phidgetMotorsLocker = new object();

        static Dumping permission = Dumping.All;
        static readonly object dumpLocker = new object();

        SerialPort robo1, robo2;

        public Receiver():this(11000)
        {
        }

        public Receiver(int port)
        {
            try
            {
                this.listenPort = port;
                shouldStop = false;
                /*open phidgets motor/servo controller*/
                lock (phidgetMotorsLocker)
                {
                    phidgetMotors = new Servo();
                    phidgetMotors.open();
                    phidgetMotors.waitForAttachment(3000);

                    for (int i = 0; i < phidgetMotors.servos.Count; i++)
                    {
                        phidgetMotors.servos[i].Engaged = false;
                        phidgetMotors.servos[i].Type = ServoServo.ServoType.RAW_us_MODE;
                        phidgetMotors.servos[i].Position = 1500;
                        phidgetMotors.servos[i].Engaged = true;
                    }
                }
                /*open phidgets 8/8/8*/
                ikit = new InterfaceKit();
                /*add watcher for the digital inputs*/
                ikit.InputChange += new InputChangeEventHandler(iKit_InputChange);
                ikit.open();
                ikit.waitForAttachment(3000);
                /*create roboteq worker*/
                roboteqs = new BackgroundWorker();
                roboteqs.DoWork += new DoWorkEventHandler(roboteqs_DoWork);
                /*open serial ports*/
                robo1 = new SerialPort("COM1", 115200, Parity.None, 8, StopBits.One);
                robo1.WriteTimeout = 100;
                robo2 = new SerialPort("COM2", 115200, Parity.None, 8, StopBits.One);
                robo2.WriteTimeout = 100;
                robo1.Open();
                robo2.Open();
            }
            catch (Exception ex)
            {
                throw;
            }
        }

       void iKit_InputChange(object sender, InputChangeEventArgs e)
        {
            switch (e.Index)
            {
                case 0: /*cancel/enable dump*/
                    lock (dumpLocker)
                    {
                        if (e.Value == false)
                            permission = Dumping.All;
                        else
                            permission = Dumping.NoReturn;
                    }
                    lock (phidgetMotorsLocker)
                        phidgetMotors.servos[1].Position = 1500;
                    break;
                case 1: /*cancel/enable return*/
                    lock (dumpLocker)
                    {
                        if (e.Value == false)
                            permission = Dumping.All;
                        else
                            permission = Dumping.NoDump;
                    }
                    lock (phidgetMotorsLocker)
                        phidgetMotors.servos[1].Position = 1500;
                    break;
            }
        }

        public void Receive()
        {
            shouldStop = false;
            /*set connection parameters*/
            UdpClient listener = new UdpClient(listenPort);
            listener.Client.ReceiveTimeout = 500;
            IPEndPoint groupEP = new IPEndPoint(IPAddress.Any, listenPort);
            byte[] buffer;
            MemoryStream bufferStream;
            XmlSerializer deserializer = new XmlSerializer(typeof(LunabotOutputState));

            while (!shouldStop)
            {
                try
                {
                    /*attempt to receive data*/
                    buffer = listener.Receive(ref groupEP);
                    /*deserialize*/
                    bufferStream = new MemoryStream(buffer);
                    LunabotOutputState state = (LunabotOutputState)deserializer.Deserialize(bufferStream);
                    roboteqs.RunWorkerAsync(state);
                }
                catch (SocketException ex)
                {
                    try
                    {
                        /*haven't received an update in time...kill all actions*/
                        LunabotOutputState nullState = new LunabotOutputState();
                        roboteqs.RunWorkerAsync(nullState);
                    }
                    catch (Exception ex2)
                    {
                    }
                }
                catch (Exception ex)
                {
                }
            }
            /*close connections*/
            robo1.Close();
            robo2.Close();
        }

        private void roboteqs_DoWork(object sender, DoWorkEventArgs e)
        {
            try
            {
                LunabotOutputState state = e.Argument as LunabotOutputState;
                /*handle phidgets operations*/

                /*handle collection bin*/
                lock(dumpLocker)
                {
                    lock(phidgetMotorsLocker)
                    {
                        switch(permission)
                        {
                            case Dumping.All:
                                phidgetMotors.servos[1].Position = (state.collectionBin > 2000) ? 2000 : ((state.collectionBin < 1000) ? 1000 : state.collectionBin);
                                break;
                            case Dumping.NoReturn:
                                phidgetMotors.servos[1].Position = (state.collectionBin > 1500) ? 1500 : ((state.collectionBin < 1000) ? 1000 : state.collectionBin);
                                break;
                            case Dumping.NoDump:
                                phidgetMotors.servos[1].Position = (state.collectionBin > 2000) ? 2000 : ((state.collectionBin < 1500) ? 1500 : state.collectionBin);
                                break;
                        }
                    }
                }

                /*handle digging mechanism*/
                lock(phidgetMotorsLocker)
                    phidgetMotors.servos[0].Position = (state.diggingMechanism > 2000) ? 2000 : ((state.diggingMechanism < 1000) ? 1000 : state.diggingMechanism);

                switch (state.diggingHead)
                {
                    case LunabotOutputState.ActionState.Forward:
                        ikit.outputs[2] = false;
                        ikit.outputs[3] = true;
                        break;
                    case LunabotOutputState.ActionState.Reverse:
                        ikit.outputs[3] = false;
                        ikit.outputs[2] = true;
                        break;
                    case LunabotOutputState.ActionState.Off:
                        ikit.outputs[2] = false;
                        ikit.outputs[3] = false;
                        break;
                }

                /*process motor values*/
                int motorLeft = (state.motorSpeedLeft < -1000) ? -1000 : ((state.motorSpeedLeft > 1000) ? 1000 : state.motorSpeedLeft);
                int motorRight = ((state.motorSpeedRight < -1000) ? -1000 : ((state.motorSpeedRight > 1000) ? 1000 : state.motorSpeedRight))*-1;
                string roboCommand = "!M " + motorLeft.ToString() + " " + motorRight.ToString() + "\r\n";
                robo1.Write(roboCommand);
                robo2.Write(roboCommand);
            }
            catch (Exception ex)
            {
            }
        }

        public void Stop()
        {
            shouldStop = true;
        }
    }
}
